#include "dynamic_hit_outpost.h"
#include "../../../tool/include/util_func.h"
#include "../../../tool/include/macros.h"
#include "../autoaim_node.hpp"


void DynamicHitOutpost::update(const InputOfDynamicOutpost &data)
{
    if (!data.armor_switched)
    {
        car_armors_data.push(data);
        same_armor_data.push(data);
        judge_rotate_dir(data.yaw_speed, outpost_description.dir);
    } 
    else 
    {
        last_armor_switch_clock = data.target.clock;
        // 记录切换装甲板的时间戳
        armor_switch_clock_recorder.push(data.target.clock);
        // 通过判断当前装甲板与上一次的装甲板的位置判断是左armor还是右armor
        InputOfDynamicOutpost last_input_of_outposter;
        if(!car_armors_data.back(last_input_of_outposter, 1))
        {
            return;
        }
        if (data.target.yaw > last_input_of_outposter.target.yaw) // 切换到左侧装甲板， 当前敌方机器人往右转动
        {
            right_switch_targets.push(last_input_of_outposter.target);
            outpost_description.right_yaw = 0.5 * outpost_description.right_yaw + 0.5 * last_input_of_outposter.target.yaw; 
            left_switch_targets.push(data.target);
            if (!flag_init_yaw) 
            { 
                outpost_description.left_yaw = data.target.yaw; 
                flag_init_yaw = true;
            }
            else 
            { 
                outpost_description.left_yaw = 0.5 * outpost_description.left_yaw + 0.5 * data.target.yaw; 
            }
        }
        else
        {// 切换到右侧装甲板, 当前敌方机器人往左转动
            left_switch_targets.push(last_input_of_outposter.target);
            outpost_description.left_yaw = 0.5 * outpost_description.left_yaw + 0.5 * last_input_of_outposter.target.yaw; 
            right_switch_targets.push(data.target);
            if (!flag_init_yaw)
            {
                outpost_description.right_yaw =  data.target.yaw;
                flag_init_yaw = true;
            }
            else 
            { 
                outpost_description.right_yaw = 0.5 * outpost_description.right_yaw + 0.5 * data.target.yaw; 
            }
        }
        // 计算陀螺有关参数
        calculate_outpost_info();
        // 清空数据
        clear_if_armor_switched();
        car_armors_data.push(data);
        same_armor_data.push(data);
        judge_rotate_dir(data.yaw_speed, outpost_description.dir);
    }


    // 计算当前装甲板与上一次切换时的时间差， 从而判断是否需要调整至无陀螺状态
// 这里需要考虑是否是刚刚初始化的车辆，last_armor_switch_clock=0 必然得是无陀螺状态
    int64_t delta_clock_bwtween_CurrClock_and_LastArmorSwitchClock = 0.0;
    delta_clock_bwtween_CurrClock_and_LastArmorSwitchClock = data.target.clock - last_armor_switch_clock;
    bool condition_1 = (delta_clock_bwtween_CurrClock_and_LastArmorSwitchClock > 2000 && car_armors_data.length > 90);
    if (condition_1)  
    {
        clear_if_car_switched();
        outpost_description.oupost_status = OutpostMotionStatus::STALBLE;
    }
    // outpost_description_mutex.unlock();
}
// 利用点进行数据拟合
cv::Mat DynamicHitOutpost::polyfit(std::vector<cv::Point2d>& in_point, int n)
{
	// 长度
	int size = in_point.size();
	//所求未知数个数
	int x_num = n + 1;
	//构造矩阵U和Y
	cv::Mat mat_u(size, x_num, CV_64F);
	cv::Mat mat_y(size, 1, CV_64F);
 
	for (int i = 0; i < mat_u.rows; ++i)
		for (int j = 0; j < mat_u.cols; ++j)
		{
			mat_u.at<double>(i, j) = pow(in_point[i].x, j);
		}
 
	for (int i = 0; i < mat_y.rows; ++i)
	{
		mat_y.at<double>(i, 0) = in_point[i].y;
	}
 
	//矩阵运算，获得系数矩阵K
	cv::Mat mat_k(x_num, 1, CV_64F);
	mat_k = (mat_u.t()*mat_u).inv()*mat_u.t()*mat_y;
	// std::cout << mat_k << std::endl;
	return mat_k;
}

double DynamicHitOutpost::getResult(cv::Mat mat_k,int n,double inNum){
    double outNum=0;
    for (int j = 0; j < n + 1; ++j)
    {
        outNum += mat_k.at<double>(j, 0)*pow(inNum,j);
    }
    return outNum;
}

double DynamicHitOutpost::solveEquations(cv::Mat mat_k,int n,double targetNum,double solution,int minNum,int maxNum){ 
	if(n==2){
         return (targetNum-mat_k.at<double>(0, 0))/mat_k.at<double>(1, 0);
    }
    double p;
    double mid;
    bool location;
    if(getResult(mat_k,n,minNum)<getResult(mat_k,n,maxNum)){
        location=true;
    }
    else{
        location=false;
    }
    while(true){  
		mid=(minNum+maxNum)/2; //二分区间 
		p=getResult(mat_k,n,mid);
		if(std::abs(p-targetNum)<solution){ //更新区间 
			return mid;
		}
		else if(location==true){
            if(p<targetNum){
                minNum=mid;
            }
            else{
                maxNum=mid;
            }
        }else{
            if(p<targetNum){
                maxNum=mid;
            }
            else{
                minNum=mid;
            }
        }
	}
}
// void DynamicHitOutpost::update_new(const InputOfDynamicOutpost &data)
// {
//     if (!data.armor_switched)
//     {
//         // 记录装甲板yaw数据
//         yaw_recording.push_back(cv::Point2d(data.target.clock,data.target.yaw));
//         // DEBUG(
//             // std::cout<<"时间："<<data.target.clock<<"yaw:"<<data.target.yaw<<"阶数矩阵："<<mat_k.size()<<std::endl;
            
//         // );
//         /*获取yaw数据输出*/
//         std::string addr = "../save/csv/getYaw.csv";
//         std::ofstream fout;
//         fout.open(addr,std::ios::app|std::ios::out);
//         fout<<data.target.clock<<",";
//         fout<<data.target.yaw<<" ";
//         fout.close();
//         /**/  
//         car_armors_data.push(data);
//         same_armor_data.push(data);
//         judge_rotate_dir(data.yaw_speed, outpost_description.dir);
//     } 
//     else 
//     {   

//         // std::cout<<"时间(切换装甲板):"<<data.target.clock<<"yaw:"<<data.target.yaw<<"阶数矩阵："<<mat_k.size()<<std::endl;
//         /*-----------------------------*/ 
//         // 装甲板切换时候必须保证有效的装甲板数量大于4，一方面是为了防止第一次装甲板切换
//         if(yaw_recording.size()>4){ 
//             if(std::abs(yaw_recording[0].y-yaw_recording[yaw_recording.size()-1].y)>90){
//                 for(unsigned int i = 0; i < yaw_recording.size(); ++i)
//                 {
//                     if(yaw_recording[i].y-yaw_recording[yaw_recording.size()-1].y<-90){
//                         yaw_recording[i].y+=360;
//                     }
//                     else if(yaw_recording[i].y-yaw_recording[yaw_recording.size()-1].y>90){
//                         yaw_recording[i].y-=360;
//                     }
//                 }
//             }
//             // 计算出有效数据
//             mat_k.push_back(polyfit(yaw_recording, jie));
//         }
//         std::cout<<yaw_recording[0].x<<std::endl;
//         std::cout<<data.target.clock<<std::endl;

//         std::cout << yaw_recording[0].x<<",";
//         std::cout <<yaw_recording[yaw_recording.size()-1].x<<"]"<<std::endl;
//         // std::cout<<"["<<yaw_recording[0].y<<","<<yaw_recording[yaw_recording.size()-1].y<<"]"<<std::endl;
//         /*-----------------------------*/       
//         last_armor_switch_clock = data.target.clock;
//         // 记录切换装甲板的时间戳
//         armor_switch_clock_recorder.push(data.target.clock);
//         // 通过判断当前装甲板与上一次的装甲板的位置判断是左armor还是右armor
//         InputOfDynamicOutpost last_input_of_outposter;
//         if(!car_armors_data.back(last_input_of_outposter, 1))
//         {
//             return;
//         }
//         if (data.target.yaw > last_input_of_outposter.target.yaw) // 切换到左侧装甲板， 当前敌方机器人往右转动
//         {
//             right_switch_targets.push(last_input_of_outposter.target);
//             outpost_description.right_yaw = 0.5 * outpost_description.right_yaw + 0.5 * last_input_of_outposter.target.yaw; 
//             left_switch_targets.push(data.target);
//             if (!flag_init_yaw) 
//             { 
//                 outpost_description.left_yaw = data.target.yaw; 
//                 flag_init_yaw = true;
//             }
//             else 
//             { 
//                 outpost_description.left_yaw = 0.5 * outpost_description.left_yaw + 0.5 * data.target.yaw; 
//             }
//         }
//         else
//         {// 切换到右侧装甲板, 当前敌方机器人往左转动
//             left_switch_targets.push(last_input_of_outposter.target);
//             outpost_description.left_yaw = 0.5 * outpost_description.left_yaw + 0.5 * last_input_of_outposter.target.yaw; 
//             right_switch_targets.push(data.target);
//             if (!flag_init_yaw)
//             {
//                 outpost_description.right_yaw =  data.target.yaw;
//                 flag_init_yaw = true;
//             }
//             else 
//             { 
//                 outpost_description.right_yaw = 0.5 * outpost_description.right_yaw + 0.5 * data.target.yaw; 
//             }
//         }
//         // 计算陀螺有关参数(这里是将装甲板信息进行清除)
//         calculate_outpost_info_new();
//         // 清空数据
//         clear_if_armor_switched();
//         yaw_recording.push_back(cv::Point2d(data.target.clock,data.target.yaw));
//         /*获取yaw数据输出*/
//         std::string addr = "../save/csv/getYaw.csv";
//         std::ofstream fout;
//         fout.open(addr,std::ios::app|std::ios::out);
//         fout<<std::endl;
//         fout<<data.target.clock<<",";
//         fout<<data.target.yaw<<" ";
//         fout.close();
//         /**/  
//         car_armors_data.push(data);
//         same_armor_data.push(data);
//         judge_rotate_dir(data.yaw_speed, outpost_description.dir);
//     }


//     // 计算当前装甲板与上一次切换时的时间差， 从而判断是否需要调整至无陀螺状态
// // 这里需要考虑是否是刚刚初始化的车辆，last_armor_switch_clock=0 必然得是无陀螺状态
//     int64_t delta_clock_bwtween_CurrClock_and_LastArmorSwitchClock = 0.0;
//     delta_clock_bwtween_CurrClock_and_LastArmorSwitchClock = data.target.clock - last_armor_switch_clock;
//     bool condition_1 = (delta_clock_bwtween_CurrClock_and_LastArmorSwitchClock > 2000 && car_armors_data.length > 90);
//     if (condition_1)  
//     {
//         clear_if_car_switched();
//         outpost_description.oupost_status = OutpostMotionStatus::STALBLE;
//     }
//     // outpost_description_mutex.unlock();
// }

double DynamicHitOutpost::fun(int n, double array[YAWLENGTH][YAWLENGTH])
{
	int ii, jj, k, u;
	int iter = 0;  //记录行变换的次数（交换）
	double det1 = 1, yin;
 
	for (ii = 0; ii < n; ii++)
	{
		for (k = ii + 1; k < n; k++)
		{
			yin = -1 * array[k][ii] / array[ii][ii];
 
			for (u = 0; u < n; u++)
			{
				array[k][u] = array[k][u] + array[ii][u] * yin;
			}
		}
	}
	for (ii = 0; ii < n; ii++)  //求对角线的积 即 行列式的值
		det1 = det1 * array[ii][ii];
	//行变换偶数次符号不变
	if (iter % 2 == 1)
		det1 = -det1;
	return (det1);
}

void  DynamicHitOutpost::LeastSquareMethod(int n,int m, double* x, double* y,double &aYaw,double &bYaw) {
	double base_time = x[0];
    int i, j;
	double temp[YAWLENGTH][YAWLENGTH];
	double d[YAWLENGTH], arr[YAWLENGTH][YAWLENGTH];
	double tempsumx, tempsumy;
	double sumx[YAWLENGTH] = { 0.0 }, sumy[YAWLENGTH] = { 0.0 };
	for (i = 0; i <2*m; i++) {
		tempsumx = 0.0;
		tempsumy = 0.0;
		for (j = 0; j < n; j++) {
			tempsumx += pow(x[j] - base_time, i + 1);
			tempsumy += pow(x[j] - base_time, i) * y[j];
		}
		sumx[i] = tempsumx;
		sumy[i] = tempsumy;
	}
	int a=0;
	//上三角
	for (i = 1; i < m + 1; i++) {
		temp[0][0] = n;
		for (j = 0; j <= i; j++) {
			temp[j][i - j] = sumx[i - 1];
			a = i - 1;
		}
	}
	//下三角
	for (j = 1; j <= m; j++) {
		for (i = 0; i <= m - j; i++) {
			temp[i + j][m - i] = sumx[a + j];
		}
	}
	//临时赋值
	for (i = 0; i <= m; i++) {
		for (j = 0; j <= m; j++) {
			arr[i][j] = temp[i][j];
		}
	}
	for (i = 0; i <= m; i++) {
		for (j = 0; j <= m; j++) {
			arr[j][i] = sumy[j];
		}
		d[i + 1] = fun(m+1, arr);//求行列式的值
		for (int p = 0; p <= m; p++) {
			for (int q = 0; q <= m; q++) {
				arr[p][q] = temp[p][q];//恢复被改变过的arr
			}
		}
	}
	d[0] = fun(m + 1, temp);
	double z[YAWLENGTH];
	for (i = 0; i <= m; i++) {
		z[i] = d[i + 1] / d[0];
	}
	char cha = '+';
	// cout << "拟合数据的" << m << "次多项式为:P" << m << "(x)=" << z[0];
	for (i = 1; i <= m; i++) {
		if (z[i] < 0)
			cha = ' ';
		// cout << cha << z[i] << "x^" << i;
	}
    aYaw=z[1];
    bYaw=z[0];
    bYaw -= aYaw * base_time;
	// cout << endl;
}
// void DynamicHitOutpost::calculateYawSave(){
    
//     Target now_left_target, now_right_target, last_left_target, last_right_target;
//     left_switch_targets.back(now_left_target, 1);
//     left_switch_targets.back(last_left_target, 2);
//     right_switch_targets.back(now_right_target, 1);
//     right_switch_targets.back(last_right_target, 2);
//     if(std::abs(centerYaw)<0.001){
//         LeastSquareMethod(lengthOfSaveYaw, 1, saveTime, saveYaw,aYaw,bYaw);
//         clearYawSave();
//         centerYaw=(now_right_target.yaw+last_left_target.yaw)/2;
//         yawSectionNew=abs(now_right_target.yaw-last_left_target.yaw);
//         timeCenter=(centerYaw-bYaw)/aYaw;
        
//     }
//     else{
//         centerYaw=0.8*(now_right_target.yaw+last_left_target.yaw)/2+0.2*centerYaw;
//         timeCenter=(centerYaw-bYaw)/aYaw;

//         DEBUGJS(printf("上一次预测中心的时间:%lf\n",timeCenter););
//         yawSectionNew=0.8*abs(now_right_target.yaw-last_left_target.yaw)+0.2*yawSectionNew;
//         double lastaYaw=aYaw;
//         LeastSquareMethod(lengthOfSaveYaw, 1, saveTime, saveYaw,aYaw,bYaw);
//         clearYawSave();
//         if(timeSection<0.01){
//             timeSection=(centerYaw-bYaw)/aYaw-timeCenter;
//         }
//         else{
//             timeSection=0.7*(centerYaw-bYaw)/aYaw-timeCenter+0.3*timeSection;
//         }
//         preTimeCenter1=2*(centerYaw-bYaw)/aYaw-timeCenter;
//         // preTimeCenter1=(centerYaw-bYaw)/aYaw+timeSection;
//         aPreYaw1=0.7*aYaw+0.3*lastaYaw;
//         bPreYaw1=centerYaw-aPreYaw1*preTimeCenter1;
//         preTimeCenter2=2*((centerYaw-bYaw)/aYaw-timeCenter)+(centerYaw-bYaw)/aYaw;
//         // preTimeCenter1=(centerYaw-bYaw)/aYaw+2*timeSection;
//         aPreYaw2=0.7*aYaw+0.3*lastaYaw;
//         bPreYaw2=centerYaw-aPreYaw2*preTimeCenter2;
        


//         timeCenter=(centerYaw-bYaw)/aYaw;
//         preTimSection[0][0]=(timeCenter+preTimeCenter1)/2;
//         preTimSection[0][1]=(preTimeCenter2+preTimeCenter1)/2;
//         preTimSection[1][0]=(preTimeCenter2+preTimeCenter1)/2;
//         preTimSection[1][1]=2*preTimeCenter2-preTimSection[1][0];
//         linpre=true;
//         DEBUGJS(
//             printf("预测的中心：%lf..........................\n",centerYaw);
//             printf("本次的时间中心：%lf",timeCenter);
//             printf("预测的时间中心：%lf\n",preTimeCenter1);
//             printf("斜率：%lf\n",aYaw);
//             printf("截距：%lf\n",bPreYaw1);
//         );
//         aYaw=0.2*lastaYaw+0.8*aYaw;
//     }

//     // //将数据输出
//     // std::ofstream fout;
//     // std::string save_addr ="../save/image/yawLin.csv";
//     // fout.open(save_addr,std::ios::app|std::ios::out);
//     // fout<< now_right_target.yaw<<",";
//     // fout<< last_left_target.yaw<<",";
//     // fout<< centerYaw<<",";
//     // fout<< yawSectionNew<<",";
//     // fout<< timeCenter<<",";
//     // fout<< aYaw<<",";
//     // fout<< bYaw<<",";
//     // fout<< preTimeCenter1<<",";
//     // fout<< aPreYaw1<<",";
//     // fout<< bPreYaw1<<",";
//     // fout<< preTimeCenter2<<",";
//     // fout<< aPreYaw2<<",";
//     // fout<< bPreYaw2<<std::endl;
//     // fout.close();

// }







void DynamicHitOutpost::update_new(InputOfDynamicOutpost &data)
{
    autoaim_node->autoaim_recorder.switch_num = 0;
    autoaim_node->autoaim_recorder.target_clock = 0;
    autoaim_node->autoaim_recorder.length_ofsaveyaw = 0;
    if(outpost_description.switchNum==0&&judgeFirst==true){
        judgeFirst=false;
        if(data.target.yaw>0||data.target.yaw<-180){
            judgeNum=1;
        }
    }
    else if(outpost_description.switchNum!=0){
        judgeFirst=true;
    }
    if(judgeNum==1){
        if(data.target.yaw<-180){
            data.target.yaw+=360.0;
        }
    }
    car_armors_data.push(data);
    autoaim_node->autoaim_recorder.switch_num = outpost_description.switchNum;
    autoaim_node->autoaim_recorder.target_clock = data.target.clock;
    autoaim_node->autoaim_recorder.length_ofsaveyaw = outpost_description.lengthOfSaveYaw;

    autoaim_node->autoaim_recorder.center_yaw = 0;
    autoaim_node->autoaim_recorder.center_z = 0;
    autoaim_node->autoaim_recorder.center_dis = 0;
    autoaim_node->autoaim_recorder.center_time0 = 0;

    if (data.armor_switched&&outpost_description.lengthOfSaveYaw>4)
    {
        
        // std::cout<<outpost_description.switchNum<<std::endl;
        if(outpost_description.switchNum==0){
            double aYaw,bYaw;
            LeastSquareMethod(outpost_description.lengthOfSaveYaw,1,outpost_description.saveTime,outpost_description.saveYaw,aYaw,bYaw);
            //第一次仅仅用来进行旋转方向的判断
            if(aYaw<0)
                outpost_description.dir=ArmorDir::RIGHT;
            else{
                outpost_description.dir=ArmorDir::LEFT;
            }    
        }
        else if(outpost_description.switchNum==1){
            // std::cout<<"重新记录yaw...."<<std::endl;
            // std::cout<<outpost_description.centerYaw<<std::endl;

            // outpost_description.centerYaw=outpost_description.saveYaw[outpost_description.lengthOfSaveYaw-1]+outpost_description.saveYaw[outpost_description.lengthOfSaveYaw-2]+outpost_description.saveYaw[outpost_description.lengthOfSaveYaw-3]+outpost_description.saveYaw[0]+outpost_description.saveYaw[1]+outpost_description.saveYaw[2];
            // outpost_description.centerYaw=outpost_description.centerYaw/6;
            outpost_description.centerYaw=(outpost_description.saveYaw[outpost_description.lengthOfSaveYaw/2]+outpost_description.saveYaw[outpost_description.lengthOfSaveYaw/2+1])/2;
            outpost_description.centerZ=(outpost_description.saveZ[outpost_description.lengthOfSaveYaw/2]+outpost_description.saveZ[outpost_description.lengthOfSaveYaw/2+1])/2;
            RCLCPP_INFO(autoaim_node->get_logger(), "centerZ: %lf", outpost_description.centerZ);
            outpost_description.centerDis=(outpost_description.saveDis[outpost_description.lengthOfSaveYaw/2]+outpost_description.saveDis[outpost_description.lengthOfSaveYaw/2+1])/2;
            // std::cout<<outpost_description.centerYaw<<std::endl;
            // std::cout<<outpost_description.centerZ<<std::endl;
            // std::cout<<outpost_description.centerDis<<std::endl;
            autoaim_node->autoaim_recorder.center_yaw = outpost_description.centerYaw;
            autoaim_node->autoaim_recorder.center_z = outpost_description.centerZ;
            autoaim_node->autoaim_recorder.center_dis = outpost_description.centerDis;
            autoaim_node->autoaim_recorder.center_time0 = outpost_description.centerTime[0];

            double aYaw,bYaw;
            LeastSquareMethod(outpost_description.lengthOfSaveYaw,1,outpost_description.saveTime,outpost_description.saveYaw,aYaw,bYaw);
            outpost_description.centerTime[0]=(outpost_description.centerYaw-bYaw)/aYaw;
        }
        else if(outpost_description.switchNum>=2){
            double aYaw,bYaw;
            LeastSquareMethod(outpost_description.lengthOfSaveYaw,1,outpost_description.saveTime,outpost_description.saveYaw,aYaw,bYaw);
            if(outpost_description.switchNum==2)
                outpost_description.delTime=(outpost_description.centerYaw-bYaw)/aYaw-outpost_description.centerTime[0];
            else 
                outpost_description.delTime=0.5*((outpost_description.centerYaw-bYaw)/aYaw-outpost_description.centerTime[0])+0.5*(outpost_description.delTime);
            
            outpost_description.centerTime[0]=(outpost_description.centerYaw-bYaw)/aYaw;
            outpost_description.centerTime[1]=outpost_description.centerTime[0]+outpost_description.delTime;
            outpost_description.centerTime[2]=outpost_description.centerTime[0]+2*outpost_description.delTime;
            outpost_description.centerTime[3]=outpost_description.centerTime[0]+3*outpost_description.delTime;


            autoaim_node->autoaim_recorder.ayaw = aYaw;
            autoaim_node->autoaim_recorder.byaw = bYaw;
            autoaim_node->autoaim_recorder.del_time_three0 = outpost_description.del_time_three[0];
            autoaim_node->autoaim_recorder.del_time_three1 = outpost_description.del_time_three[1];
            autoaim_node->autoaim_recorder.del_time_three2 = outpost_description.del_time_three[2];
            autoaim_node->autoaim_recorder.center_time0 = outpost_description.centerTime[0];
            autoaim_node->autoaim_recorder.center_time1 = outpost_description.centerTime[1];
            autoaim_node->autoaim_recorder.center_time2 = outpost_description.centerTime[2];
            autoaim_node->autoaim_recorder.center_time3 = outpost_description.centerTime[3];

            //centerTime.csv
            /*--------------------------*/ 
            // std::ofstream fout;
            // std::string save_addr = "../save/csv/centerTime.csv";
            // fout.open(save_addr, std::ios::app | std::ios::out);
            // if (!fout.is_open()) { std::cout << "unvalid save_addr" << std::endl; }
            
            // fout << outpost_description.delTime << ",";
            // fout << outpost_description.centerTime[0] << ",";
            // fout << outpost_description.centerTime[1] << ",";
            // fout << outpost_description.centerTime[2] << ",";
            // fout << outpost_description.centerTime[3] << std::endl;

            // fout.close();
            /*--------------------------*/ 
            outpost_description.valid=true;
            // std::cout<<"this true!"<<std::endl;
        }
        outpost_description.switchNum++;
        // 清零重新记录
        outpost_description.lengthOfSaveYaw=0;
        outpost_description.saveYaw[outpost_description.lengthOfSaveYaw]=data.target.yaw;
        outpost_description.saveTime[outpost_description.lengthOfSaveYaw]=data.target.clock;
        outpost_description.saveDis[outpost_description.lengthOfSaveYaw]=data.target.xy_plane_distance;
        outpost_description.saveZ[outpost_description.lengthOfSaveYaw]=data.target.z;

        outpost_description.lengthOfSaveYaw++;
        // 这里对data数据进行了修改
        data.armor_switched=false;
    }
    else if(data.armor_switched){

        
        // 清零重新记录
        outpost_description.lengthOfSaveYaw=0;
        outpost_description.saveYaw[outpost_description.lengthOfSaveYaw]=data.target.yaw;
        outpost_description.saveTime[outpost_description.lengthOfSaveYaw]=data.target.clock;
        outpost_description.lengthOfSaveYaw++;
    }
    else{
        //线性击打 保存区间之内的yaw数值  1-13
        if(outpost_description.lengthOfSaveYaw<100){
            outpost_description.saveYaw[outpost_description.lengthOfSaveYaw]=data.target.yaw;
            outpost_description.saveTime[outpost_description.lengthOfSaveYaw]=data.target.clock;
            outpost_description.saveDis[outpost_description.lengthOfSaveYaw]=data.target.xy_plane_distance;
            outpost_description.saveZ[outpost_description.lengthOfSaveYaw]=data.target.z;
            outpost_description.lengthOfSaveYaw++;
        }
    }
}

// 动态击打旋转前哨站数据更新
void DynamicHitOutpost::update_new_dynatic(InputOfDynamicOutpost &data)
{
    autoaim_node->autoaim_recorder.switch_num = 0;
    autoaim_node->autoaim_recorder.target_clock = 0;
    autoaim_node->autoaim_recorder.length_ofsaveyaw = 0;


    // std::string s = vaddr;
    // std::string addr = "../save/csv/dy_switch_info_"+s;
    // std::ofstream fout;
    // fout.open(addr,std::ios::app|std::ios::out);
    // fout<<"outpost_description.centerTime[0]:"<<outpost_description.centerTime[0]<<",";

    if(outpost_description.switchNum==0&&judgeFirst==true){
        judgeFirst=false;
        if(data.target.yaw>0||data.target.yaw<-180){
            judgeNum=1;
        }
    }
    else if(outpost_description.switchNum!=0){
        judgeFirst=true;
    }
    if(judgeNum==1){
        if(data.target.yaw<-180){
            data.target.yaw+=360.0;
        }
    }
    car_armors_data.push(data);
    autoaim_node->autoaim_recorder.switch_num = outpost_description.switchNum;
    autoaim_node->autoaim_recorder.target_clock = data.target.clock;
    autoaim_node->autoaim_recorder.length_ofsaveyaw = outpost_description.lengthOfSaveYaw;
    // DEBUG_IP(
    //     if(data.armor_switched){
    //         fout<<"次数:"<<outpost_description.switchNum<<",";
    //         fout<<"时间:"<< data.target.clock<<",";
    //         fout<<"长度:"<<outpost_description.lengthOfSaveYaw<<",";
    //     }
    // );
    // 判断到装甲板切换（如果数据小于4就会被舍弃）
    autoaim_node->autoaim_recorder.center_yaw = 0;
    autoaim_node->autoaim_recorder.center_z = 0;
    autoaim_node->autoaim_recorder.center_dis = 0;
    autoaim_node->autoaim_recorder.center_time0 = 0;

    if (data.armor_switched&&outpost_description.lengthOfSaveYaw>4)//切换装甲板
    {
        // 第一次切换装甲板（只用来判断旋转方向）
        if(outpost_description.switchNum==0){
            double aYaw,bYaw;
            LeastSquareMethod(outpost_description.lengthOfSaveYaw,1,outpost_description.saveTime,outpost_description.saveYaw,aYaw,bYaw);
            if(aYaw<0){
                outpost_description.dir=ArmorDir::RIGHT;
                // DEBUG_IP(
                //     fout<<"右转"<<",";
                // );
            }
            else{
                outpost_description.dir=ArmorDir::LEFT;
                // DEBUG_IP(
                //     fout<<"左转"<<",";
                // );
            }
        }
        else if(outpost_description.switchNum==1){
            if(center_switch==true){
                // std::cout<<"重新记录yaw...."<<std::endl;
                outpost_description.centerYaw=(outpost_description.saveYaw[outpost_description.lengthOfSaveYaw/2]+outpost_description.saveYaw[outpost_description.lengthOfSaveYaw/2+1])/2;
                outpost_description.centerZ=(outpost_description.saveZ[outpost_description.lengthOfSaveYaw/2]+outpost_description.saveZ[outpost_description.lengthOfSaveYaw/2+1])/2;
                outpost_description.centerDis=(outpost_description.saveDis[outpost_description.lengthOfSaveYaw/2]+outpost_description.saveDis[outpost_description.lengthOfSaveYaw/2+1])/2;
                // DEBUG_IP(
                //     fout<<"中心yaw:"<<outpost_description.centerYaw<<",";
                //     fout<<"中心Z:"<<outpost_description.centerZ<<",";
                //     fout<<"中心距离:"<<outpost_description.centerDis<<",";
                //     fout<<"中心时间:"<<outpost_description.centerTime[0]<<",";
                // );
                // 可以更改间隔时间
                autoaim_node->autoaim_recorder.center_yaw = outpost_description.centerYaw;
                autoaim_node->autoaim_recorder.center_z = outpost_description.centerZ;
                autoaim_node->autoaim_recorder.center_dis = outpost_description.centerDis;
                autoaim_node->autoaim_recorder.center_time0 = outpost_description.centerTime[0];
                
                change_dy_time=true;
                center_switch=false;
            }
            // DEBUG_IP(
            // std::cout<<"中心yaw:"<<outpost_description.centerYaw<<std::endl;
            // std::cout<<"中心z:"<<outpost_description.centerZ<<std::endl;
            // std::cout<<"中心距离："<<outpost_description.centerDis<<std::endl;
            // );
            double aYaw,bYaw;
            LeastSquareMethod(outpost_description.lengthOfSaveYaw,1,outpost_description.saveTime,outpost_description.saveYaw,aYaw,bYaw);
            outpost_description.centerTime[0]=(outpost_description.centerYaw-bYaw)/aYaw;
            outpost_description.aYaw=aYaw;
        }
        else if(outpost_description.switchNum>=2){

            if (3 == autoaim_node->car_id)
            {
                double rate = 0.3;
                double tmp_centerYaw = 0, tmp_centerZ = 0, tmp_centerDis = 0;
                tmp_centerYaw = (outpost_description.saveYaw[outpost_description.lengthOfSaveYaw/2]+outpost_description.saveYaw[outpost_description.lengthOfSaveYaw/2+1])/2;
                tmp_centerZ = (outpost_description.saveZ[outpost_description.lengthOfSaveYaw/2]+outpost_description.saveZ[outpost_description.lengthOfSaveYaw/2+1])/2;
                tmp_centerDis=(outpost_description.saveDis[outpost_description.lengthOfSaveYaw/2]+outpost_description.saveDis[outpost_description.lengthOfSaveYaw/2+1])/2;
                outpost_description.centerYaw = rate * tmp_centerYaw + (1 - rate) * outpost_description.centerYaw;
                outpost_description.centerZ = rate * tmp_centerZ + (1 - rate) * outpost_description.centerZ;
                outpost_description.centerDis = rate * tmp_centerDis + (1 - rate) * outpost_description.centerDis;
            }

            double aYaw,bYaw;
            LeastSquareMethod(outpost_description.lengthOfSaveYaw,1,outpost_description.saveTime,outpost_description.saveYaw,aYaw,bYaw);
            
            double del_time_this=(outpost_description.centerYaw-bYaw)/aYaw-outpost_description.centerTime[0];
            // fout<<"del_time_this"<<",";
            // fout<<"减去的中心时间："<<outpost_description.centerTime[0]<<",";
            autoaim_node->autoaim_recorder.center_time0 = outpost_description.centerTime[0];

            int title_switch=1;
            if(del_time_this>1000){
                title_switch=2;
            } 
            del_time_this=del_time_this/title_switch;

            // fout<<"本次认为切换的次数："<<title_switch<<",";
            autoaim_node->autoaim_recorder.title_switch = title_switch;
            outpost_description.switchNum--;
            for(int i=0;i<title_switch;i++){
                outpost_description.switchNum++;
                if(i==0)bYaw+=del_time_this*aYaw*(title_switch-1);
                else{
                    bYaw=bYaw-del_time_this*aYaw;
                }
                double last_centerTime_temp=outpost_description.centerTime[0];
                // if(i==0)last_centerTime_temp+=(title_switch-1)*del_time_this;
                double pre_centerTime_temp=outpost_description.centerTime[1];
                // if(i==0)pre_centerTime_temp+=(title_switch-1)*del_time_this;
                if(outpost_description.switchNum<=3){
                    // outpost_description.delTime=(outpost_description.centerYaw-bYaw)/aYaw-outpost_description.centerTime[0];
                    if(outpost_description.switchNum==2){
                        outpost_description.delTime=del_time_this;
                    }
                    else{
                        outpost_description.delTime=0.5*(del_time_this)+0.5*(outpost_description.delTime);
                    }
                    outpost_description.del_time_three[(outpost_description.switchNum+1)%3]=del_time_this;
                    // std::cout<<"outpost_description.delTime"<<outpost_description.delTime<<std::endl;
                    RCLCPP_INFO(autoaim_node->get_logger(), "outpost_description.delTime: %f", outpost_description.delTime);
                    outpost_description.centerTime[0]=(outpost_description.centerYaw-bYaw)/aYaw;
                    outpost_description.centerTime[1]=outpost_description.centerTime[0]+outpost_description.delTime;
                    outpost_description.centerTime[2]=outpost_description.centerTime[0]+2*outpost_description.delTime;
                    outpost_description.centerTime[3]=outpost_description.centerTime[0]+3*outpost_description.delTime;
                }
                else {
                    if(outpost_description.switchNum==4){
                        outpost_description.del_time_three[2]=del_time_this;
                    }
                    else{
                        outpost_description.del_time_three[(outpost_description.switchNum+1)%3]=0.5*( outpost_description.del_time_three[(outpost_description.switchNum+1)%3])+0.5*(del_time_this);
                    }
                    outpost_description.centerTime[0]=(outpost_description.centerYaw-bYaw)/aYaw;
                    outpost_description.centerTime[1]=outpost_description.centerTime[0]+outpost_description.del_time_three[(outpost_description.switchNum+2)%3];
                    outpost_description.centerTime[2]=outpost_description.centerTime[1]+outpost_description.del_time_three[(outpost_description.switchNum)%3];
                    outpost_description.centerTime[3]=outpost_description.centerTime[2]+outpost_description.del_time_three[(outpost_description.switchNum+1)%3];
                    // outpost_description.delTime=0.5*((outpost_description.centerYaw-bYaw)/aYaw-outpost_description.centerTime[0])+0.5*(outpost_description.delTime);
                }
                if(change_dy_time==true&&std::abs(pre_centerTime_temp-outpost_description.centerTime[0])<10){
                    change_dy_time=false;
                    outpost_description.pre_del_time=pre_centerTime_temp-last_centerTime_temp;
                }
                if(std::abs(pre_centerTime_temp-outpost_description.centerTime[0])<10){
                    outpost_description.centerTime[4]=outpost_description.centerTime[0];
                }
                // DEBUG_IP(
                //     fout<<"中心yaw："<<outpost_description.centerYaw<<",";
                //     fout<<"bYaw："<<bYaw<<",";
                //     fout<<"ayaw："<<aYaw<<",";

                //     fout<<"区间1："<<outpost_description.del_time_three[0]<<",";
                //     fout<<"区间2："<<outpost_description.del_time_three[1]<<",";
                //     fout<<"区间3："<<outpost_description.del_time_three[2]<<",";

                //     fout <<"预测误差:"<<pre_centerTime_temp-outpost_description.centerTime[0]<< ",";
                //     fout <<"本次斜率:"<<aYaw<< ",";
                //     fout <<"实际本次间隔时长:"<<outpost_description.centerTime[0]- last_centerTime_temp << ",";
                //     fout <<"预测间隔时长:"<< outpost_description.centerTime[1]-outpost_description.centerTime[0] << ",";
                //     fout <<"本次中心时间:"<< outpost_description.centerTime[0] << ",";
                //     fout <<"下次中心:"<< outpost_description.centerTime[1] << ",";
                //     fout <<"下下次:"<< outpost_description.centerTime[2] << ",";
                //     fout <<"下下下次:"<< outpost_description.centerTime[3] << ",";
                // );
                // 记录斜率数值
                autoaim_node->autoaim_recorder.ayaw = aYaw;
                autoaim_node->autoaim_recorder.byaw = bYaw;
                autoaim_node->autoaim_recorder.del_time_three0 = outpost_description.del_time_three[0];
                autoaim_node->autoaim_recorder.del_time_three1 = outpost_description.del_time_three[1];
                autoaim_node->autoaim_recorder.del_time_three2 = outpost_description.del_time_three[2];
                autoaim_node->autoaim_recorder.center_time0 = outpost_description.centerTime[0];
                autoaim_node->autoaim_recorder.center_time1 = outpost_description.centerTime[1];
                autoaim_node->autoaim_recorder.center_time2 = outpost_description.centerTime[2];
                autoaim_node->autoaim_recorder.center_time3 = outpost_description.centerTime[3];

                outpost_description.aYaw=0.5*aYaw+0.5*outpost_description.aYaw;
                outpost_description.valid=true;
            }
        }
        outpost_description.switchNum++;
        // 清零重新记录
        outpost_description.lengthOfSaveYaw=0;
        outpost_description.saveYaw[outpost_description.lengthOfSaveYaw]=data.target.yaw;
        outpost_description.saveTime[outpost_description.lengthOfSaveYaw]=data.target.clock;
        outpost_description.lengthOfSaveYaw++;
        // 这里对data数据进行了修改
        data.armor_switched=false;

        // DEBUG_IP(
        //     fout<<std::endl;
        // );  
        
    }
    else if(data.armor_switched){
        // DEBUG_IP(
        //     fout<<std::endl;
        // );  
        // 清零重新记录
        outpost_description.lengthOfSaveYaw=0;
        outpost_description.saveYaw[outpost_description.lengthOfSaveYaw]=data.target.yaw;
        outpost_description.saveTime[outpost_description.lengthOfSaveYaw]=data.target.clock;
        outpost_description.saveDis[outpost_description.lengthOfSaveYaw]=data.target.xy_plane_distance;           
        outpost_description.saveZ[outpost_description.lengthOfSaveYaw]=data.target.z;
        outpost_description.lengthOfSaveYaw++;
    }
    else{//未切换
        //线性击打 保存区间之内的yaw数值  1-13
        if(outpost_description.lengthOfSaveYaw<100){
            // fout<<"时间："<<data.target.clock<<","<<"yaw:"<<data.target.yaw<<std::endl;

            outpost_description.saveYaw[outpost_description.lengthOfSaveYaw]=data.target.yaw;
            outpost_description.saveTime[outpost_description.lengthOfSaveYaw]=data.target.clock;
            outpost_description.saveDis[outpost_description.lengthOfSaveYaw]=data.target.xy_plane_distance;
            outpost_description.saveZ[outpost_description.lengthOfSaveYaw]=data.target.z;
            outpost_description.lengthOfSaveYaw++;
        }
    }

    
}




void DynamicHitOutpost::clear_if_car_switched()
{
    // outpost_description_mutex.lock();
    same_armor_data.clear();
    car_armors_data.clear();
    left_switch_targets.clear();
    right_switch_targets.clear();
    armor_switch_clock_recorder.clear();
    outpost_description.reset();
    last_armor_switch_clock = 0;   
    flag_init_yaw = false;
    flag_init_average_yaw = false;
    judgeNum=0;
    judgeFirst=true;
    // outpost_description_mutex.unlock();
}

void DynamicHitOutpost::clear_if_armor_switched()
{
    same_armor_data.clear();
    yaw_recording.clear();
}
void DynamicHitOutpost::calculate_outpost_info_new()
{
    if (left_switch_targets.length < 2) return;

    if(mat_k.size()==2){
        centerYaw=(yaw_recording[yaw_recording.size()-1].y+yaw_recording[yaw_recording.size()-2].y+yaw_recording[yaw_recording.size()-3].y+yaw_recording[0].y+yaw_recording[1].y+yaw_recording[2].y)/6;
        std::cout<<"中心yaw："<<centerYaw<<std::endl;
        InputOfDynamicOutpost centerInfo;
        same_armor_data.back(centerInfo,same_armor_data.length/2);
        centerTarget=centerInfo.target;
        
        // 获取精度容忍限制
        double solution=Util::getYawResolution(centerTarget, false);
        centerPoints.push_back(cv::Point2d(solveEquations(mat_k[mat_k.size()-1],jie,centerYaw,solution/4,yaw_recording[0].x,yaw_recording[yaw_recording.size()-1].x),centerYaw));
    }
    else if(mat_k.size()>2){
        InputOfDynamicOutpost centerInfo;
        same_armor_data.back(centerInfo,same_armor_data.length/2);
        centerTarget=centerInfo.target;
        // 获取精度容忍限制
        double solution=Util::getYawResolution(centerTarget, false);
        centerPoints.push_back(cv::Point2d(solveEquations(mat_k[mat_k.size()-1],jie,centerYaw,solution/4,yaw_recording[0].x,yaw_recording[yaw_recording.size()-1].x),centerYaw));
        delTime+=((centerPoints[centerPoints.size()-1].x-centerPoints[centerPoints.size()-2].x)-delTime)/(double)(mat_k.size()-2);
        prTime[0]=centerPoints[centerPoints.size()-1].x+delTime;
        prTime[1]=centerPoints[centerPoints.size()-1].x+2*delTime;
        prTime[2]=centerPoints[centerPoints.size()-1].x+3*delTime;
        fireBegin=true;
    }


    Target now_left_target, now_right_target, last_left_target, last_right_target;
    left_switch_targets.back(now_left_target, 1);
    left_switch_targets.back(last_left_target, 2);
    right_switch_targets.back(now_right_target, 1);
    right_switch_targets.back(last_right_target, 2);

    double delta_yaw = 0.5 * ((now_right_target.yaw + now_left_target.yaw) - (last_right_target.yaw + last_left_target.yaw));
    double delta_time = static_cast<double>(0.5 * ((now_right_target.clock + now_left_target.clock) - (last_right_target.clock + last_left_target.clock)));

    // 计算观测的前哨战旋转周期
    int64_t curr_armor_switch_clock_ = 0, last_armor_switch_clock_ = 0;
    armor_switch_clock_recorder.back(curr_armor_switch_clock_, 1);
    armor_switch_clock_recorder.back(last_armor_switch_clock_, 2);
    outpost_description.observe_period = 0.5 * outpost_description.observe_period + 0.001 * 0.5 * (curr_armor_switch_clock_ - last_armor_switch_clock_) * armor_num;
    // get_exporter("observe_period.txt")->export_info(0.001 * (curr_armor_switch_clock_ - last_armor_switch_clock_) * armor_num);
    if (std::abs(outpost_description.observe_period - outpost_period) > 0.5)
    {
        outpost_description.observe_period = outpost_period;
        // LOGE("compute outpost period failed, use default data");
        RCLCPP_INFO(autoaim_node->get_logger(), "compute outpost period failed, use default data");
    }

    // 计算区间周期
    get_time_span(outpost_description.time_span);
    
    // 计算敌方机器人左右两侧装甲板对应的yaw角度
    outpost_description.yaw_section = 0.5 * outpost_description.yaw_section + 0.5 * std::abs(now_left_target.yaw - now_right_target.yaw); 
    // 计算敌方机器人中心相对于我方机器人移动的yaw角速度
    outpost_description.car_yaw_speed = 0.0;
    // 判断陀螺状态
    judge_outpost_status_when_armor_switched(now_left_target.xy_plane_distance);
    // 计算装甲板平均z轴高度
    outpost_description.average_armor_z_axis = 0.5 * outpost_description.average_armor_z_axis + 0.5 * 0.5 * (now_left_target.z + now_right_target.z);
    // 计算平均yaw角度
    if (!flag_init_average_yaw)
     {
         outpost_description.average_yaw =  0.5 * (now_left_target.yaw + now_right_target.yaw);
         flag_init_average_yaw = true;
     }
     else
    { 
        outpost_description.average_yaw = 0.5 * outpost_description.average_yaw + 0.5 * 0.5 * (now_left_target.yaw + now_right_target.yaw);
     }

     if (outpost_description.dir != ArmorDir::UNKNOWN) { outpost_description.valid = true; }
}





void DynamicHitOutpost::calculate_outpost_info()
{
    if (left_switch_targets.length < 2) return;
    Target now_left_target, now_right_target, last_left_target, last_right_target;
    left_switch_targets.back(now_left_target, 1);
    left_switch_targets.back(last_left_target, 2);
    right_switch_targets.back(now_right_target, 1);
    right_switch_targets.back(last_right_target, 2);

    double delta_yaw = 0.5 * ((now_right_target.yaw + now_left_target.yaw) - (last_right_target.yaw + last_left_target.yaw));
    double delta_time = static_cast<double>(0.5 * ((now_right_target.clock + now_left_target.clock) - (last_right_target.clock + last_left_target.clock)));

    // 计算观测的前哨战旋转周期
    int64_t curr_armor_switch_clock_ = 0, last_armor_switch_clock_ = 0;
    armor_switch_clock_recorder.back(curr_armor_switch_clock_, 1);
    armor_switch_clock_recorder.back(last_armor_switch_clock_, 2);
    outpost_description.observe_period = 0.5 * outpost_description.observe_period + 0.001 * 0.5 * (curr_armor_switch_clock_ - last_armor_switch_clock_) * armor_num;
    // get_exporter("observe_period.txt")->export_info(0.001 * (curr_armor_switch_clock_ - last_armor_switch_clock_) * armor_num);
    if (std::abs(outpost_description.observe_period - outpost_period) > 0.5)
    {
        outpost_description.observe_period = outpost_period;
        RCLCPP_INFO(autoaim_node->get_logger(), "compute observe period failed, use default data");
        // LOGE("compute outpost period failed, use default data");
    }

    // 计算区间周期
    get_time_span(outpost_description.time_span);
    
    // 计算敌方机器人左右两侧装甲板对应的yaw角度
    outpost_description.yaw_section = 0.5 * outpost_description.yaw_section + 0.5 * std::abs(now_left_target.yaw - now_right_target.yaw); 
    // 计算敌方机器人中心相对于我方机器人移动的yaw角速度
    outpost_description.car_yaw_speed = 0.0;
    // 判断陀螺状态
    judge_outpost_status_when_armor_switched(now_left_target.xy_plane_distance);
    // 计算装甲板平均z轴高度
    outpost_description.average_armor_z_axis = 0.5 * outpost_description.average_armor_z_axis + 0.5 * 0.5 * (now_left_target.z + now_right_target.z);
    // 计算平均yaw角度
    if (!flag_init_average_yaw)
     {
         outpost_description.average_yaw =  0.5 * (now_left_target.yaw + now_right_target.yaw);
         flag_init_average_yaw = true;
     }
     else
    { 
        outpost_description.average_yaw = 0.5 * outpost_description.average_yaw + 0.5 * 0.5 * (now_left_target.yaw + now_right_target.yaw);
     }

     if (outpost_description.dir != ArmorDir::UNKNOWN) { outpost_description.valid = true; }
}




void DynamicHitOutpost::judge_outpost_status_when_armor_switched(double distance)
{
    if (left_switch_targets.length >= 2) 
    {
        outpost_description.oupost_status = OutpostMotionStatus::GYRO;
    }
}

void DynamicHitOutpost::follow_hit_outpost(double v, int64_t now, SendPack &send_pack)
{
    // outpost_description_mutex.lock();
    if (!outpost_description.valid) // 陀螺状态没有计算出来
    {
        // hit_median(v, now, send_pack);
        send_pack.target_found = false;
        send_pack.fire = false;
        // outpost_description_mutex.unlock();
        return;
    }
    // 最近更新的一次装甲板
    InputOfDynamicOutpost latest_data;
    car_armors_data.back(latest_data, 1);
    
    send_pack.target_found = true;
    send_pack.yaw_resolution = Util::getYawResolution(latest_data.target, true);
    send_pack.pitch_resolution = Util::getPitchResolution(latest_data.target);


    // 陀螺模型预测时间为子弹飞行时间与（当前时刻-陀螺模型最近armor_switch更新时间）的和 , 单位s
    // 由clock_now计算时间差，单位s
    auto delta_clock = static_cast<double>(now - last_armor_switch_clock) / 1000.0;
    // 根据最近的一帧装甲板后验观测数据计算子弹飞行时间
    double flight_time = 0.0;
    if (!Util::computBulletFlightTime(get_average_xy_distancce(), get_average_z_distancce(), v, flight_time))
    {
        send_pack.target_found = false;
        send_pack.fire = false;
        RCLCPP_INFO(autoaim_node->get_logger(), "get flight time error!");
        // LOGW("get flight time error!");
        // outpost_description_mutex.unlock();
        return;
    }
    double predict_time = flight_time + delta_clock;
    // 根据陀螺模型直接预测子弹打过去的敌方车辆装甲板处于的yaw位置，计算相对于边界装甲板实际的小数部分即可
    double t_ratio = static_cast<double>(armor_num * predict_time / outpost_description.observe_period) - static_cast<int>(armor_num * predict_time / outpost_description.observe_period);
    // 计算区间所占比例
    double max_origin_ratio = armor_num * outpost_description.time_span / outpost_description.observe_period;
    if (max_origin_ratio == 0)
    {
        send_pack.target_found = false;
        send_pack.fire = false;
        return;
    }
    if (t_ratio > max_origin_ratio){
        t_ratio = 0;
    }    
    double delta_yaw = outpost_description.yaw_section * t_ratio / max_origin_ratio; // 注意要用120度*比例


    // 计算yaw, 注意yaw左边大，右边小
    if (outpost_description.dir == ArmorDir::LEFT) // 往左转
    {
        Target latest_right_target;
        right_switch_targets.back(latest_right_target, 1);
        send_pack.pred_yaw = outpost_description.right_yaw + delta_yaw;
        send_pack.yaw_palstance = 1 * armor_num * outpost_description.yaw_section / outpost_description.time_span;
        auto pred_pitch_ = Util::getPitchForPTZUnderGravity(get_average_xy_distancce(), get_average_z_distancce(), v);
        send_pack.pred_pitch = pred_pitch_.value_or(send_pack.pred_pitch);// 如果计算失败则使用旧值
    }
    else
    {
        Target latest_left_target;
        left_switch_targets.back(latest_left_target, 1);
        send_pack.pred_yaw = outpost_description.left_yaw - delta_yaw;
        send_pack.yaw_palstance = -1 * armor_num * outpost_description.yaw_section / outpost_description.time_span;
        auto pred_pitch_ = Util::getPitchForPTZUnderGravity(get_average_xy_distancce(), get_average_z_distancce(), v);
        send_pack.pred_pitch = pred_pitch_.value_or(send_pack.pred_pitch);// 如果计算失败则使用旧值
    }


    // 考虑云台在回枪时理论上击打不中目标，因此选择自动发弹，考虑发弹延时
    double predict_time_with_shoot_delay = flight_time + delta_clock + shoot_delay;
    double t_ratio_with_shoot_delay = static_cast<double>(armor_num * predict_time_with_shoot_delay / outpost_description.observe_period) - static_cast<int>(armor_num * predict_time_with_shoot_delay / outpost_description.observe_period);
    if (t_ratio_with_shoot_delay > max_origin_ratio)
    {
        t_ratio_with_shoot_delay = 0;
    }
    t_ratio_with_shoot_delay = t_ratio_with_shoot_delay / max_origin_ratio;
    if (std::abs(t_ratio_with_shoot_delay - 0.4) < 0.1)
    {
        send_pack.fire = true;
    } else
    {
        send_pack.fire = false;
    }
}


void DynamicHitOutpost::aim_center_hit_outpost(double v, int64_t now, SendPack &send_pack)
{
    // outpost_description_mutex.lock();
    if (!outpost_description.valid) // 陀螺状态没有计算出来
    {
        // hit_median(v, now, send_pack);
        send_pack.target_found = false;
        send_pack.fire = false;
        // outpost_description_mutex.unlock();
        return;
    }
    // 最近更新的一次装甲板
    InputOfDynamicOutpost latest_data;
    car_armors_data.back(latest_data, 1);
    
    send_pack.target_found = true;
    send_pack.yaw_resolution = Util::getYawResolution(latest_data.target, true);
    send_pack.pitch_resolution = Util::getPitchResolution(latest_data.target);


    // 陀螺模型预测时间为子弹飞行时间与（当前时刻-陀螺模型最近armor_switch更新时间）的和 , 单位s
    // 由clock_now计算时间差，单位s
    auto delta_clock = static_cast<double>(now - last_armor_switch_clock) / 1000.0;
    // 根据最近的一帧装甲板后验观测数据计算子弹飞行时间
    double flight_time = 0.0;
    if (!Util::computBulletFlightTime(get_average_xy_distancce(), get_average_z_distancce(), v, flight_time))
    {
        send_pack.target_found = false;
        send_pack.fire = false;
        // LOGW("get flight time error!");
        RCLCPP_INFO(autoaim_node->get_logger(), "get flight time error!");
        // outpost_description_mutex.unlock();
        return;
    }

    send_pack.pred_yaw = outpost_description.average_yaw;
    auto pred_pitch_ = Util::getPitchForPTZUnderGravity(get_average_xy_distancce(), get_average_z_distancce(), v);
    send_pack.pred_pitch = pred_pitch_.value_or(send_pack.pred_pitch);// 如果计算失败则使用旧值
    send_pack.yaw_palstance = 0.0;
    send_pack.pitch_palstance = 0.0;

    double max_origin_ratio = armor_num * outpost_description.time_span / outpost_description.observe_period;
    if (max_origin_ratio == 0)
    {
        send_pack.target_found = false;
        send_pack.fire = false;
        return;
    }
    // 考虑云台在回枪时理论上击打不中目标，因此选择自动发弹，考虑发弹延时
    double predict_time_with_shoot_delay = flight_time + delta_clock + shoot_delay;
    double t_ratio_with_shoot_delay = static_cast<double>(armor_num * predict_time_with_shoot_delay / outpost_description.observe_period) - static_cast<int>(armor_num * predict_time_with_shoot_delay / outpost_description.observe_period);
    if (t_ratio_with_shoot_delay > max_origin_ratio)
    {
        t_ratio_with_shoot_delay = 0;
    }
    t_ratio_with_shoot_delay = t_ratio_with_shoot_delay / max_origin_ratio;
    if (std::abs(t_ratio_with_shoot_delay - 0.5) < 0.5 * Util::getYawResolution(latest_data.target, false) / outpost_description.yaw_section)
    {
        send_pack.fire = true;
    } else
    {
        send_pack.fire = false;
    }
}

bool DynamicHitOutpost::rightTime(int64_t now,double flyTime){
    flyTime+=dynatic_shoot_delay;
    if(std::abs(outpost_description.centerTime[1]-now-flyTime)<10||std::abs(outpost_description.centerTime[2]-now-flyTime)<10||std::abs(outpost_description.centerTime[3]-now-flyTime)<10){
        // std::cout<<"now:"<<now<<std::endl;
        // std::cout<<"fly:"<<flyTime<<std::endl;
        // std::cout<<now+flyTime<<std::endl;
        // std::cout<<outpost_description.centerTime[1]<<std::endl;
        // std::cout<<outpost_description.centerTime[2]<<std::endl;
        // std::cout<<outpost_description.centerTime[3]<<std::endl;
        return true;
    }
    else{
        return false;
    }
}

int DynamicHitOutpost::getTimes(int64_t now,double flyTime,bool right_info){
    flyTime+=dynatic_shoot_delay;
    // if(std::abs(outpost_description.centerTime[1]-now-flyTime)<outpost_description.delTime/2)return 1;
    // else if(std::abs(outpost_description.centerTime[2]-now-flyTime)<outpost_description.delTime/2)return 2;
    // else if(std::abs(outpost_description.centerTime[3]-now-flyTime)<outpost_description.delTime/2)return 3;
    // else return -1;
    if(std::abs(outpost_description.centerTime[1]-outpost_description.centerTime[0]-outpost_description.pre_del_time)>100&&right_info&&std::abs(now+flyTime-(outpost_description.centerTime[4]+outpost_description.pre_del_time*((int)((int)(now+flyTime-outpost_description.centerTime[4])/(int)(outpost_description.pre_del_time/2.0)+1)/2)))<dynatic_shoot_proportion*outpost_description.pre_del_time){
        return -2;
    }
    else{
        if(std::abs(outpost_description.centerTime[1]-now-flyTime)<(outpost_description.centerTime[1]-outpost_description.centerTime[0])*dynatic_shoot_proportion)return 1;
        else if(std::abs(outpost_description.centerTime[2]-now-flyTime)<(outpost_description.centerTime[2]-outpost_description.centerTime[1])*dynatic_shoot_proportion)return 2;
        else if(std::abs(outpost_description.centerTime[3]-now-flyTime)<(outpost_description.centerTime[3]-outpost_description.centerTime[2])*dynatic_shoot_proportion)return 3;
        else return -1;
    }

    // if(std::abs(outpost_description.centerTime[1]-now-flyTime)<5||std::abs(outpost_description.centerTime[2]-now-flyTime)<5||std::abs(outpost_description.centerTime[3]-now-flyTime)<5){
    //     std::cout<<"now:"<<now<<std::endl;
    //     std::cout<<"fly:"<<flyTime<<std::endl;
    //     std::cout<<now+flyTime<<std::endl;
    //     std::cout<<outpost_description.centerTime[1]<<std::endl;
    //     std::cout<<outpost_description.centerTime[2]<<std::endl;
    //     std::cout<<outpost_description.centerTime[3]<<std::endl;
    //     return true;
    // }
    // else{
    //     return false;
    // }
}


void DynamicHitOutpost::aim_center_hit_outpost_new(double v, int64_t now, SendPack &send_pack)
{
    // outpost_description_mutex.lock();
    // if (!outpost_description.valid) // 陀螺状态没有计算出来
    // {
    //     // hit_median(v, now, send_pack);
    //     send_pack.target_found = false;
    //     send_pack.fire = false;
    //     // outpost_description_mutex.unlock();
    //     return;
    // }
    // 最近更新的一次装甲板
    InputOfDynamicOutpost latest_data;
    car_armors_data.back(latest_data, 1);
    
    send_pack.target_found = true;
    send_pack.yaw_resolution = Util::getYawResolution(latest_data.target, false);
    send_pack.pitch_resolution = Util::getPitchResolution(latest_data.target);


    // 陀螺模型预测时间为子弹飞行时间与（当前时刻-陀螺模型最近armor_switch更新时间）的和 , 单位s
    // 由clock_now计算时间差，单位s
    auto delta_clock = static_cast<double>(now - last_armor_switch_clock) / 1000.0;
    // 根据最近的一帧装甲板后验观测数据计算子弹飞行时间
    double flight_time = 0.0;
    // std::cout<<"get_average_xy_distancce:"<<get_average_xy_distancce()<<","<<get_average_z_distancce()<<std::endl;
    if (!Util::computBulletFlightTime(outpost_description.centerDis, outpost_description.centerZ, v, flight_time))
    {
        send_pack.target_found = false;
        send_pack.fire = false;
        // LOGW("get flight time error!");
        RCLCPP_INFO(autoaim_node->get_logger(), "get flight time error!");
        // outpost_description_mutex.unlock();
        return;
    }

    // send_pack.pred_yaw = outpost_description.average_yaw;
    send_pack.pred_yaw = outpost_description.centerYaw;
    // std::cout<<get_average_xy_distancce()<<","<<get_average_z_distancce()<<std::endl;
    auto pred_pitch_ = Util::getPitchForPTZUnderGravity(outpost_description.centerDis, outpost_description.centerZ, v);
    send_pack.pred_pitch = pred_pitch_.value_or(send_pack.pred_pitch);// 如果计算失败则使用旧值

    send_pack.yaw_palstance = 0.0;
    send_pack.pitch_palstance = 0.0;

    cv::FileStorage file;
    file.open(g_tune_param_path, cv::FileStorage::READ);
    file["dynatic_shoot_delay"] >> dynatic_shoot_delay;
    file.release();
    
    if(rightTime(now,flight_time*1000)){
        send_pack.fire = true;
        send_pack.target_found = true;
        // send_pack.burstShoot = false;
        
    }
    else{
        send_pack.target_found = true;
        send_pack.fire = false;
        // send_pack.burstShoot = false;
    }
    // double max_origin_ratio = armor_num * outpost_description.time_span / outpost_description.observe_period;
    // if (max_origin_ratio == 0)
    // {
    //     send_pack.target_found = false;
    //     send_pack.fire = false;
    //     return;
    // }
    // // 考虑云台在回枪时理论上击打不中目标，因此选择自动发弹，考虑发弹延时
    // double predict_time_with_shoot_delay = flight_time + delta_clock + shoot_delay;
    // double t_ratio_with_shoot_delay = static_cast<double>(armor_num * predict_time_with_shoot_delay / outpost_description.observe_period) - static_cast<int>(armor_num * predict_time_with_shoot_delay / outpost_description.observe_period);
    // if (t_ratio_with_shoot_delay > max_origin_ratio)
    // {
    //     t_ratio_with_shoot_delay = 0;
    // }
    // t_ratio_with_shoot_delay = t_ratio_with_shoot_delay / max_origin_ratio;
    // if (std::abs(t_ratio_with_shoot_delay - 0.5) < 0.5 * Util::getYawResolution(latest_data.target, false) / outpost_description.yaw_section)
    // {
    //     send_pack.fire = true;
    // } else
    // {
    //     send_pack.fire = false;
    // }
}




void DynamicHitOutpost::follow_hit_outpost_new(double v, int64_t now, SendPack &send_pack)
// void DynamicHitOutpost::aim_center_hit_outpost_new(double v, int64_t now, SendPack &send_pack)
{
    // outpost_description_mutex.lock();
    // if (!outpost_description.valid) // 陀螺状态没有计算出来
    // {
    //     // hit_median(v, now, send_pack);
    //     send_pack.target_found = false;
    //     send_pack.fire = false;
    //     // outpost_description_mutex.unlock();
    //     return;
    // }
    // 最近更新的一次装甲板
    InputOfDynamicOutpost latest_data;
    car_armors_data.back(latest_data, 1);
    
    send_pack.target_found = true;
    send_pack.yaw_resolution = Util::getYawResolution(latest_data.target, false);
    send_pack.pitch_resolution = Util::getPitchResolution(latest_data.target);


    // 陀螺模型预测时间为子弹飞行时间与（当前时刻-陀螺模型最近armor_switch更新时间）的和 , 单位s
    // 由clock_now计算时间差，单位s
    auto delta_clock = static_cast<double>(now - last_armor_switch_clock) / 1000.0;
    // 根据最近的一帧装甲板后验观测数据计算子弹飞行时间
    double flight_time = 0.0;
    if (!Util::computBulletFlightTime(get_average_xy_distancce(), get_average_z_distancce(), v, flight_time))
    {
        send_pack.target_found = false;
        send_pack.fire = false;
        // LOGW("get flight time error!");
        RCLCPP_INFO(autoaim_node->get_logger(), "get flight time error!");
        // outpost_description_mutex.unlock();
        return;
    }

    // send_pack.pred_yaw = outpost_description.average_yaw;
    // send_pack.pred_yaw = outpost_description.centerYaw;

    auto pred_pitch_ = Util::getPitchForPTZUnderGravity(outpost_description.centerDis ,outpost_description.centerZ, v);
    send_pack.pred_pitch = pred_pitch_.value_or(send_pack.pred_pitch);// 如果计算失败则使用旧值
    // if(CAR_ID==7)send_pack.pred_pitch += 2;
    send_pack.yaw_palstance = 0.0;
    send_pack.pitch_palstance = 0.0;

    // if(rightTime(now,flight_time*1000)){
    //     send_pack.fire = true;
    //     send_pack.target_found = true;
    //     // send_pack.burstShoot = false;
        
    // }
    // else{
    //     send_pack.target_found = true;
    //     send_pack.fire = false;
    //     // send_pack.burstShoot = false;
    // }
    // std::string s = vaddr;
    // std::string addr = "../save/csv/dy_send_info_"+s;
    // std::ofstream fout;
    // fout.open(addr,std::ios::app|std::ios::out);

    cv::FileStorage file;
    file.open(g_tune_param_path, cv::FileStorage::READ);
    file["dynatic_shoot_delay"] >> dynatic_shoot_delay;
    file.release();


    int numTime=getTimes(now,flight_time*1000,(now-outpost_description.centerTime[4])<8000&&std::abs(outpost_description.pre_del_time)>200);
    // fout<<"当前时间："<<now<<",";
    // fout<<"选择情况:"<<numTime<<std::endl;
    // fout<<"是否超时:"<<now-outpost_description.centerTime[4]<<",";
    // fout<<"预测时间间隔:"<<outpost_description.pre_del_time<<",";
    // RCLCPP_INFO(autoaim_node->get_logger(), "numTime: %d", numTime);
    if(numTime==-2){
        send_pack.pred_yaw=outpost_description.centerYaw+outpost_description.aYaw*(now+dynatic_shoot_delay+flight_time*1000-(outpost_description.centerTime[4]+outpost_description.pre_del_time*((int)((int)(now+dynatic_shoot_delay+flight_time*1000-outpost_description.centerTime[4])/(int)(outpost_description.pre_del_time/2.0)+1)/2)));
        // fout<<"中心yaw:"<<outpost_description.centerYaw<<",";
        // RCLCPP_INFO(autoaim_node->get_logger(), "numTime==-2");
    }
    else if(numTime!=-1){
        send_pack.fire = true;
        send_pack.target_found = true;
        // send_pack.pred_yaw = outpost_description.centerYaw+outpost_description.aYaw*(now+dynatic_shoot_delay+flight_time*1000-numTime*outpost_description.delTime-outpost_description.centerTime[0]);
        send_pack.pred_yaw=outpost_description.centerYaw+outpost_description.aYaw*(now+dynatic_shoot_delay+flight_time*1000-outpost_description.centerTime[numTime]);
        // RCLCPP_INFO(autoaim_node->get_logger(), "numTime==-1");
        // send_pack.burstShoot = false;
    }
    else{
        send_pack.target_found = true;
        send_pack.fire = false;
        send_pack.pred_yaw = outpost_description.centerYaw+outpost_description.aYaw*(-1*outpost_description.delTime*dynatic_shoot_proportion);
        // RCLCPP_INFO(autoaim_node->get_logger(), "else");
    }
    // fout<<"预测yaw："<<send_pack.pred_yaw<<std::endl;


    // double max_origin_ratio = 3 * outpost_description.time_span / outpost_description.observe_period;
    // if (max_origin_ratio == 0)
    // {
    //     send_pack.target_found = false;
    //     send_pack.fire = false;
    //     return;
    // }
    // // 考虑云台在回枪时理论上击打不中目标，因此选择自动发弹，考虑发弹延时
    // double predict_time_with_shoot_delay = flight_time + delta_clock + shoot_delay;
    // double t_ratio_with_shoot_delay = static_cast<double>(3 * predict_time_with_shoot_delay / outpost_description.observe_period) - static_cast<int>(3 * predict_time_with_shoot_delay / outpost_description.observe_period);
    // if (t_ratio_with_shoot_delay > max_origin_ratio)
    // {
    //     t_ratio_with_shoot_delay = 0;
    // }
    // t_ratio_with_shoot_delay = t_ratio_with_shoot_delay / max_origin_ratio;
    // if (std::abs(t_ratio_with_shoot_delay - 0.5) < 0.5 * Util::getYawResolution(latest_data.target, false) / outpost_description.yaw_section)
    // {
    //     send_pack.fire = true;
    // } else
    // {
    //     send_pack.fire = false;
    // }

    //回枪处理（测试版）
    // double predict_time = delta_clock + flight_time + dynatic_shoot_delay / 1000;
    // double t_ratio = static_cast<double>(predict_time / (outpost_description.delTime / 1000)) - static_cast<int>(predict_time / (outpost_description.delTime / 1000));
    // // RCLCPP_INFO(autoaim_node->get_logger(), "t_ratio: %lf", t_ratio);
    // if (t_ratio < 0.3)
    // {
    //     send_pack.fire = false;
    // }
    // else
    // {
    //     send_pack.fire = true;
    // }

}





bool DynamicHitOutpost::if_outpost_description_updated(int64_t now)
{
    if ((now - last_armor_switch_clock) > 2500)
    {
        return false;
    } else {
        return true;
    }
}


void DynamicHitOutpost::show_result(cv::Mat &canvas, double v, int64_t now)
{
    int width = canvas.cols;
    int height = canvas.rows;    

    if (this->outpost_description.oupost_status == OutpostMotionStatus::STALBLE)
    {
        cv::putText(canvas, "stable outpost!", cv::Point(420, 20), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 255, 0));
    } else if (this->outpost_description.oupost_status == OutpostMotionStatus::UNKNOWN)
    {
        cv::putText(canvas, "unknown outpost mode!", cv::Point(420, 20), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 255, 0));
    } else{
        cv::putText(canvas, "rotate!", cv::Point(420, 20), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 255, 0));
    }
    // 展示陀螺转向
    if (this->outpost_description.dir == ArmorDir::LEFT)
    {
        cv::putText(canvas, "dir: left", cv::Point(420, 40), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 255, 0));
    } else if (this->outpost_description.dir == ArmorDir::UNKNOWN)
    {
        cv::putText(canvas, "dir: unknown", cv::Point(420, 40), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 255, 0));
    } else {
        cv::putText(canvas, "dir: right", cv::Point(420, 40), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 255, 0));
    }

    if (this->outpost_description.oupost_status == OutpostMotionStatus::GYRO && left_switch_targets.length > 0)
    {
        Target left_target, right_target;
        left_switch_targets.back(left_target, 1);
        right_switch_targets.back(right_target, 1);
        // 展示左侧和右侧的线
        cv::Point2i left_point, right_point;
        Util::back_projection(left_target, left_point, left_target.pose);
        Util::back_projection(right_target, right_point, right_target.pose);
        cv::line(canvas, cv::Point2i(left_point.x, 0), cv::Point2i(left_point.x, height), cv::Scalar(0, 255, 0), 1);
        cv::line(canvas, cv::Point2i(right_point.x, 0), cv::Point2i(right_point.x, height), cv::Scalar(0, 255, 0), 1);

    } 
    // 计算当前帧的预测
    SendPack send_pack_;
    if (this->outpost_description.oupost_status == OutpostMotionStatus::GYRO)
    {
        this->follow_hit_outpost(v, now, send_pack_);
        InputOfDynamicOutpost lasted_data;
        if (!car_armors_data.back(lasted_data, 1))
        {
            return;
        }

        Target posterior_target_ = lasted_data.target; 
        // 单位度
        posterior_target_.yaw = send_pack_.pred_yaw + 90;
        posterior_target_.x =
            posterior_target_.xy_plane_distance * cos(posterior_target_.yaw * Util::PI / 180.0);
        posterior_target_.y =
            posterior_target_.xy_plane_distance * sin(posterior_target_.yaw * Util::PI / 180.0);
        cv::Point2i predict_point;
        Util::back_projection(posterior_target_, predict_point, lasted_data.target.pose);
        cv::circle(canvas, predict_point, 10, cv::Scalar(255, 255, 0), -1, cv::LINE_8);
    }
}


double DynamicHitOutpost::get_average_xy_distancce()
{
    double average_xy_distance = 0;
    for (int i = car_armors_data.head; i != car_armors_data.tail; i = ((i+1)%car_armors_data.size))
    {
        average_xy_distance = average_xy_distance + car_armors_data.values[i].target.xy_plane_distance;
    }
    return (average_xy_distance / car_armors_data.length);
}

double DynamicHitOutpost::get_average_z_distancce()
{
    double average_z_distance = 0;
    for (int i = car_armors_data.head; i != car_armors_data.tail; i = ((i+1)%car_armors_data.size))
    {
        average_z_distance = average_z_distance + car_armors_data.values[i].target.z;
    }
    return (average_z_distance / car_armors_data.length);
}

double DynamicHitOutpost::get_average_same_armor_z_distancce()
{
    double same_armor_z_distancce = 0;
    for (int i = same_armor_data.head; i != same_armor_data.tail; i = ((i+1)%same_armor_data.size))
    {
        same_armor_z_distancce = same_armor_z_distancce + same_armor_data.values[i].target.z;
    }
    return (same_armor_z_distancce / same_armor_data.length);
}


void DynamicHitOutpost::get_time_span(double &time_span)
{
    if (same_armor_data.length < 2)
    {
        // LOGE("error!"); 
        RCLCPP_ERROR(autoaim_node->get_logger(), "error!");
        time_span = time_span;
        return;
    }
    int64_t start_clock = same_armor_data.values[same_armor_data.head].target.clock;
    int64_t end_clock = same_armor_data.values[(same_armor_data.tail + same_armor_data.size - 1)%same_armor_data.size].target.clock;
    time_span = 0.5 * time_span + 0.5 * (end_clock - start_clock) * 0.001;
}

void DynamicHitOutpost::judge_rotate_dir(double yaw_speed, ArmorDir &dir)
{
    const int cnt_thresh = 3;
    static int left_dir_cnt = 0;
    if (yaw_speed > 0){
        ++left_dir_cnt;
    } else if (yaw_speed < 0)
    {
        --left_dir_cnt;
    }
    if (left_dir_cnt > cnt_thresh){
        dir = ArmorDir::LEFT;
    } else if (left_dir_cnt < -cnt_thresh){    
        dir = ArmorDir::RIGHT;
    } else {
        dir = ArmorDir::UNKNOWN;
    }
    if (left_dir_cnt > 5)
    {
        left_dir_cnt = 5; 
    } else if (left_dir_cnt < -5)
    {
        left_dir_cnt = -5;
    }
}